#include <Arduino.h>
#include <Servo.h>
#include <DFRobot_BC20_Gravity.h>
#include <HX711.h>

DFRobot_BC20_IIC myBC20(0x33);

Servo servo_R_door; //创建一个舵机类
Servo servo_K_door; //创建一个舵机类
Servo servo_T_door; //创建一个舵机类
Servo servo_O_door; //创建一个舵机类

#define USE_IIC

/*7 colors are available*/
#define RED 0
#define BLUE 1
#define GREEN 2
#define YELLOW 3
#define PURPLE 4
#define CYAN 5
#define WHITE 6

#define SERVO_R_PIN 13 //宏定义舵机控制引脚
#define SERVO_K_PIN 44 //宏定义舵机控制引脚
#define SERVO_T_PIN 3  //宏定义舵机控制引脚
#define SERVO_O_PIN 8  //宏定义舵机控制引脚

//超声波传感器一号
#define trigPin_1 15 //ultrasonic_1_trig引脚定义
#define echoPin_1 14 //ultrasonic_1_echo引脚定义

//超声波传感器二号
#define trigPin_2 7 //ultrasonic_2_trig引脚定义
#define echoPin_2 6 //ultrasonic_2_echo引脚定义

//超声波传感器三号
#define trigPin_3 11 //ultrasonic_3_trig引脚定义
#define echoPin_3 10 //ultrasonic_3_echo引脚定义

//超声波传感器四号
#define trigPin_4 5 //ultrasonic_4_trig引脚定义
#define echoPin_4 4 //ultrasonic_4_echo引脚定义

//可回收垃圾重量检测传感器引脚定义
uint8_t dataPin_R = A8;
uint8_t clockPin_R = 12;

//厨余垃圾重量检测传感器引脚定义
uint8_t dataPin_K = A9;
uint8_t clockPin_K = 46;

//有害垃圾重量检测传感器引脚定义
uint8_t dataPin_T = A10;
uint8_t clockPin_T = 45;

//其它垃圾重量检测传感器引脚定义
uint8_t dataPin_O = A11;
uint8_t clockPin_O = 9;

HX711 scale_R;
HX711 scale_K;
HX711 scale_T;
HX711 scale_O;

float calibration_factor = 511; // this calibration factor must be adjusted according to your load cell
float units_R;
float units_K;
float units_T;
float units_O;

long Ultrasonic_1_duration; //1号垃圾箱时间计算
int Ultrasonic_1_distance;  //1号垃圾箱检测高度
int Overflow_1_state;       //1号垃圾箱装载情况
int bins_1_number;          //1号垃圾箱装载次数
long Ultrasonic_2_duration; //2号垃圾箱时间计算
int Ultrasonic_2_distance;  //2号垃圾箱检测高度
int Overflow_2_state;       //2号垃圾箱装载情况
int bins_2_number;          //2号垃圾箱装载次数
long Ultrasonic_3_duration; //3号垃圾箱时间计算
int Ultrasonic_3_distance;  //3号垃圾箱检测高度
int Overflow_3_state;       //3号垃圾箱装载情况
int bins_3_number;          //3号垃圾箱装载次数
long Ultrasonic_4_duration; //4号垃圾箱时间计算
int Ultrasonic_4_distance;  //4号垃圾箱检测高度
int Overflow_4_state;       //4号垃圾箱装载情况
int bins_4_number;          //4号垃圾箱装载次数

int bins_1_distancemax;
int bins_2_distancemax;
int bins_3_distancemax;
int bins_4_distancemax;

int bins_1_duration;
int bins_2_duration;
int bins_3_duration;
int bins_4_duration;
int count;
char Bins_state; //垃圾类型识别结果

String Data;
String Time;
String GPS_LO;
String GPS_LA;

byte variable[80];
byte index = 0;
int value[5];
int chances = 1;

/*Configure device certificate information*/
char *EMQ_id = "bins"; //填入EMQ_id
char *Client_ID = "BC20";
char *EMQ_pwd = "abc123"; //填入EMQ_pwd

/*Configure the domain name and port number*/
char *EMQ_SERVER = "212.64.6.46";
char *PORT = "1883";

//实时垃圾容量监测上报
char *pubTopic = "ZhanJiang_bins";

//每日垃圾回收情况上报
char *pubTopic_weight = "ZhanJiang_bins_weight";

//实时垃圾容量监测订阅
char *subTopic = "ZhanJiang_bins";

//延时函数
void waiting(int timesNum)
{
    delay(timesNum * 1000);
}

//获取当前时间，经纬度
void Display_Location_Information()
{
    /* Check whether the data is legal */
    myBC20.changeColor(PURPLE);
    while (myBC20.getCLK())
    {
        if (sCLK.Year > 2000)
        {
            break;
        }
        Serial.print(".");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);
    }

    myBC20.getQGNSSRD2();

    //Date
    Data = (String)sCLK.Year + "/" + (String)sCLK.Month + "/" + (String)sCLK.Day;
    //Time
    Time = (String)(int(sCLK.Hour) + 8) + ":" + (String)sCLK.Minute + ":" + (String)sCLK.Second;
    //Longitude
    GPS_LO = sGGNS.LongitudeVal;
    //Latitude
    GPS_LA = sGGNS.LatitudeVal;

    myBC20.clearGPS();

    if ((int(sCLK.Hour) + 8) == 23)
    {
        Daily_weight_statistics_mqttPublish();
    }
}

//连接EMQ，订阅Topic
void ConnectCloud()
{
    Serial.print("Attempting MQTT connection...");
    myBC20.changeColor(YELLOW);
    while (!myBC20.connected())
    {
        Serial.print(".");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);

        if (myBC20.connect(Client_ID, EMQ_id, EMQ_pwd))
        {
            Serial.println("\nConnect Server OK");
        }
        else
        {
            /**
         Used to detect the connection between the device and the server
      */
            if (myBC20.getQMTCONN())
                break;
        }

        while (!myBC20.subTopic('0', '1', subTopic, '1'))
        {
            Serial.print(".");
        }
        Serial.print("\nTopic:");
        Serial.print(subTopic);
        Serial.println(" subscribed!");
        Serial.println("The module will be working ...");
    }
}

//MQTT发送函数:实时容量检测
void mqttPublish()
{
    Display_Location_Information();

    // 构建一个 JSON 格式的payload的字符串
    String payload = "{";
    payload += "\"Data\":";
    payload += "\"";
    payload += Data;
    payload += "\"";
    payload += ",";
    payload += "\"Time\":";
    payload += "\"";
    payload += Time;
    payload += "\"";
    payload += ",";
    payload += "\"deviceID\":";
    payload += "\"";
    payload += String(2021001);
    payload += "\"";
    payload += ",";
    payload += "\"GPS_LO\":";
    payload += "\"";
    payload += GPS_LO;
    payload += "\"";
    payload += ",";
    payload += "\"GPS_LA\":";
    payload += "\"";
    payload += GPS_LA;
    payload += "\"";
    payload += ",";
    payload += "\"RecWaste\":";
    payload += "\"";
    payload += String(Bin_1(1));
    payload += "\"";
    payload += ",";
    payload += "\"KitWaste\":";
    payload += "\"";
    payload += String(Bin_2(1));
    payload += "\"";
    payload += ",";
    payload += "\"ToxWaste\":";
    payload += "\"";
    payload += String(Bin_3(1));
    payload += "\"";
    payload += ",";
    payload += "\"OthWaste\":";
    payload += "\"";
    payload += String(Bin_4(1));
    payload += "\"";
    payload += "}";
    char attributes[payload.length() + 1];
    payload.toCharArray(attributes, payload.length() + 1);
    myBC20.publish(pubTopic, attributes);
    Serial.print("实时容量检测数据：[publish]-->>");
    Serial.println(attributes);

    myBC20.clearGPS();

    myBC20.LED_ON();
    delay(500);
    myBC20.LED_OFF();
}

//MQTT发送函数：每日垃圾回收量
void Daily_weight_statistics_mqttPublish()
{

    // 构建一个 JSON 格式的payload的字符串
    String payload = "{";
    payload += "\"Data\":";
    payload += "\"";
    payload += Data;
    payload += "\"";
    payload += ",";
    payload += "\"Time\":";
    payload += "\"";
    payload += Time;
    payload += "\"";
    payload += ",";
    payload += "\"deviceID\":";
    payload += "\"";
    payload += String(2021001);
    payload += "\"";
    payload += ",";
    payload += "\"GPS_LO\":";
    payload += "\"";
    payload += GPS_LO;
    payload += "\"";
    payload += ",";
    payload += "\"GPS_LA\":";
    payload += "\"";
    payload += GPS_LA;
    payload += "\"";
    payload += ",";
    payload += "\"RecWaste_weight\":";
    payload += "\"";
    payload += String(HX711_Bins(1));
    payload += "\"";
    payload += ",";
    payload += "\"KitWaste_weight\":";
    payload += "\"";
    payload += String(HX711_Bins(2));
    payload += "\"";
    payload += ",";
    payload += "\"ToxWaste_weight\":";
    payload += "\"";
    payload += String(HX711_Bins(3));
    payload += "\"";
    payload += ",";
    payload += "\"OthWaste_weight\":";
    payload += "\"";
    payload += String(HX711_Bins(4));
    payload += "\"";
    payload += "}";
    char attributes[payload.length() + 1];
    payload.toCharArray(attributes, payload.length() + 1);
    myBC20.publish(pubTopic_weight, attributes);
    Serial.print("当日各类型垃圾重量统计数据：[publish]-->>");
    Serial.println(attributes);

    myBC20.clearGPS();

    myBC20.LED_ON();
    delay(500);
    myBC20.LED_OFF();
}

//初始化BC20
void BC20_Init()
{
    myBC20.LED_OFF();

    /*Initialize BC20*/
    Serial.print("Starting the BC20.Please wait. . . ");
    myBC20.changeColor(RED);
    while (!myBC20.powerOn())
    {
        Serial.print(".");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);
    }
    Serial.println("BC20 started successfully!");

    /*Check whether SIM card is inserted*/
    Serial.println("Checking SIM card ...");
    myBC20.changeColor(GREEN);
    while (!myBC20.checkNBCard())
    {
        Serial.println("Please insert the NB SIM card !");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);
    }
    Serial.println("SIM card check OK!");

    //Disable sleep mode
    myBC20.configSleepMode(eSleepMode_Disable);
    //Disable PSM
    myBC20.setPSMMode(ePSM_OFF);

    /*Print IMEI, ICCID and IMSI*/
    myBC20.getGSN(IMEI);
    Serial.print("BC20 IMEI: ");
    Serial.println(sGSN.imei);
    Serial.print("SIM card ICCID:");
    Serial.print(myBC20.getQCCID());
    Serial.print("SIM card IMSI: ");
    Serial.println((char *)myBC20.getIMI());

    /* Disable sleep mode */
    myBC20.configSleepMode(eSleepMode_Disable);

    /*Power up GNSS*/
    Serial.print("Turning on GNSS ... ");
    myBC20.setQGNSSC(ON);
    myBC20.changeColor(YELLOW);
    if (myBC20.getQGNSSC() == OFF)
    {
        Serial.print(".");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);
    }
    Serial.println("GNSS is ON.");
    myBC20.changeColor(CYAN);

    /**
     The module will automatically attempt to connect to the network (mobile station).
     Check whether it is connected to the network.
  */
    Serial.println("Connecting network ...");
    myBC20.changeColor(BLUE);
    while (myBC20.getGATT() == 0)
    {
        Serial.print(".");
        myBC20.LED_ON();
        delay(500);
        myBC20.LED_OFF();
        delay(500);
    }
    Serial.println("Network is connected!");

    Serial.println("Connecting to EMQ");

    //Configure IoT Server
    myBC20.setServer(EMQ_SERVER, PORT);
    Serial.println("Server is available!");
    ConnectCloud();
}

//设置垃圾箱重量测量函数
int HX711_Bins(int num_hx711)
{
    units_R = scale_R.get_units(10);
    units_K = scale_K.get_units(10);
    units_T = scale_T.get_units(10);
    units_O = scale_O.get_units(10);

    if (units_R <= 0)
    {
        units_R = 0.00;
    }
    if (units_K <= 0)
    {
        units_K = 0.00;
    }
    if (units_T <= 0)
    {
        units_T = 0.00;
    }
    if (units_O <= 0)
    {
        units_O = 0.00;
    }

    switch (num_hx711)
    {
    // 可回收垃圾
    case 1:
    {
        return units_R;
    }
    break;

    // 厨余垃圾
    case 2:
    {
        return units_K;
    }
    break;

    // 有害垃圾
    case 3:
    {
        return units_T;
    }
    break;

    // 其他垃圾
    case 4:
    {
        return units_O;
    }
    break;

    default:
        break;
    }
}

//设置云台控制函数
void ServoControl_OPEN(int num_cloud)
{
    switch (num_cloud)
    {
    // 可回收垃圾
    case 1:
    {
        servo_R_door.writeMicroseconds(map(90, 0, 180, 500, 2500));
    }
    break;

    // 厨余垃圾
    case 2:
    {
        servo_K_door.writeMicroseconds(map(90, 0, 180, 500, 2500));
    }
    break;

    // 有害垃圾
    case 3:
    {
        servo_T_door.writeMicroseconds(map(90, 0, 180, 500, 2500));
    }
    break;

    // 其他垃圾
    case 4:
    {
        servo_O_door.writeMicroseconds(map(90, 0, 270, 500, 2500));
    }
    break;

    default:
        break;
    }
}

//设置云台控制函数
void ServoControl_CLOSE(int num_cloud)
{
    switch (num_cloud)
    {
    // 可回收垃圾
    case 1:
    {
        servo_R_door.writeMicroseconds(map(0, 0, 180, 500, 2500));
    }
    break;

    // 厨余垃圾
    case 2:
    {
        servo_K_door.writeMicroseconds(map(0, 0, 180, 500, 2500));
    }
    break;

    // 有害垃圾
    case 3:
    {
        servo_T_door.writeMicroseconds(map(0, 0, 180, 500, 2500));
    }
    break;

    // 其他垃圾
    case 4:
    {
        servo_O_door.writeMicroseconds(map(0, 0, 270, 500, 2500));
    }
    break;

    default:
        break;
    }
}

//定义一号垃圾装载区的容量检测函数
//"1"计算垃圾装载高度
//"2"检测是否满溢:“0”未满，“1”满溢
int Bin_1(int bin_1_num)
{
    digitalWrite(trigPin_1, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_1, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_1, LOW);
    Ultrasonic_1_duration = pulseIn(echoPin_1, HIGH);
    Ultrasonic_1_distance = (Ultrasonic_1_duration / 2) / 29.1;

    //distance<10满溢
    if (Ultrasonic_1_distance <= (bins_1_distancemax * 0.25))
    {
        Overflow_1_state = 1;
        Serial2.print("click b1,1");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }
    else
    {
        Overflow_1_state = 0;
        Serial2.print("click b1,0");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }

    switch (bin_1_num)
    {
    case 1:
    {
        return Ultrasonic_1_distance;
    }
    break;
    case 2:
    {
        return Overflow_1_state;
    }
    break;
    default:
        break;
    }
}

//定义二号垃圾装载区的容量检测函数
//"1"计算垃圾装载高度
//"2"检测是否满溢:“0”未满，“1”满溢
int Bin_2(int bin_2_num)
{
    digitalWrite(trigPin_2, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_2, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_2, LOW);
    Ultrasonic_2_duration = pulseIn(echoPin_2, HIGH);
    Ultrasonic_2_distance = (Ultrasonic_2_duration / 2) / 29.1;

    //distance<10满溢
    if (Ultrasonic_2_distance <= (bins_2_distancemax * 0.25))
    {
        Overflow_2_state = 1;
        Serial2.print("click b2,1");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }
    else
    {
        Overflow_2_state = 0;
        Serial2.print("click b2,0");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }

    switch (bin_2_num)
    {
    case 1:
    {
        return Ultrasonic_2_distance;
    }
    break;
    case 2:
    {
        return Overflow_2_state;
    }
    break;
    default:
        break;
    }
}

//定义三号垃圾装载区的容量检测函数
//"1"计算垃圾装载高度
//"2"检测是否满溢:“0”未满，“1”满溢
int Bin_3(int bin_3_num)
{
    digitalWrite(trigPin_3, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_3, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_3, LOW);
    Ultrasonic_3_duration = pulseIn(echoPin_3, HIGH);
    Ultrasonic_3_distance = (Ultrasonic_3_duration / 2) / 29.1;

    //distance<10满溢
    if (Ultrasonic_3_distance <= (bins_3_distancemax * 0.25))
    {
        Overflow_3_state = 1;
        Serial2.print("click b3,1");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }
    else
    {
        Overflow_3_state = 0;
        Serial2.print("click b3,0");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }

    switch (bin_3_num)
    {
    case 1:
    {
        return Ultrasonic_3_distance;
    }
    break;
    case 2:
    {
        return Overflow_3_state;
    }
    break;
    default:
        break;
    }
}

//定义四号垃圾装载区的容量检测函数
//"1"计算垃圾装载高度
//"2"检测是否满溢:“0”未满，“1”满溢
int Bin_4(int bin_4_num)
{
    digitalWrite(trigPin_4, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_4, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_4, LOW);
    Ultrasonic_4_duration = pulseIn(echoPin_4, HIGH);
    Ultrasonic_4_distance = (Ultrasonic_4_duration / 2) / 29.1;

    //distance<10满溢
    if (Ultrasonic_4_distance <= (bins_4_distancemax * 0.25))
    {
        Overflow_4_state = 1;
        Serial2.print("click b4,1");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }
    else
    {
        Overflow_4_state = 0;
        Serial2.print("click b4,0");
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }

    switch (bin_4_num)
    {
    case 1:
    {
        return Ultrasonic_4_distance;
    }
    break;
    case 2:
    {
        return Overflow_4_state;
    }
    break;
    default:
        break;
    }
}

void SendData()
{
    Serial2.print("t3.txt=\"");
    Serial2.print(String(count)); //显示序号
    Serial2.print("\"");
    Serial2.write(0XFF);
    Serial2.write(0XFF);
    Serial2.write(0XFF);

    Serial2.print("t11.txt=\"");
    Serial2.print(String(bins_1_number)); //可回收垃圾数量
    Serial2.print("\"");
    Serial2.write(0XFF);
    Serial2.write(0XFF);
    Serial2.write(0XFF);

    Serial2.print("t16.txt=\"");
    Serial2.print(String(bins_2_number)); //厨余垃圾数量
    Serial2.print("\"");
    Serial2.write(0XFF);
    Serial2.write(0XFF);
    Serial2.write(0XFF);

    Serial2.print("t20.txt=\"");
    Serial2.print(String(bins_3_number)); //有害垃圾数量
    Serial2.print("\"");
    Serial2.write(0XFF);
    Serial2.write(0XFF);
    Serial2.write(0XFF);

    Serial2.print("t24.txt=\"");
    Serial2.print(String(bins_4_number)); //其它垃圾数量
    Serial2.print("\"");
    Serial2.write(0XFF);
    Serial2.write(0XFF);
    Serial2.write(0XFF);

    Bin_1(2);
    Bin_2(2);
    Bin_3(2);
    Bin_4(2);
}

// 定义接收串口指令操作
void runCmd(int(numbers))
{
    //垃圾类型判断
    switch (numbers)
    {
    //接收r
    case 114:
    {
        if (Bin_1(2) == 0)
        {
            bins_1_number = bins_1_number + 1;
            SendData();
            Serial2.print("p0.pic=1"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"OK\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            ServoControl_OPEN(1);
            waiting(1);
            ServoControl_CLOSE(1);
        }
        else
        {
            SendData();
            Serial2.print("p0.pic=1"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"NO\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            waiting(2);
            //可回收垃圾已满
        }
    }
    break;
    //接收k
    case 107:
    {
        if (Bin_2(2) == 0)
        {
            bins_2_number = bins_2_number + 1;
            SendData();
            Serial2.print("p0.pic=2"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"OK\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            ServoControl_OPEN(2);
            waiting(1);
            ServoControl_CLOSE(2);
        }
        else
        {
            SendData();
            Serial2.print("p0.pic=2"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"NO\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            waiting(2);
            //厨余垃圾已满
        }
    }
    break;
    //接收t
    case 116:
    {
        if (Bin_3(2) == 0)
        {
            bins_3_number = bins_3_number + 1;
            SendData();
            Serial2.print("p0.pic=3"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"OK\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            ServoControl_OPEN(3);
            waiting(1);
            ServoControl_CLOSE(3);
        }
        else
        {
            SendData();
            Serial2.print("p0.pic=3"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"NO\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            waiting(2);
            //有害垃圾已满
        }
    }
    break;
    //接收o
    case 111:
    {
        if (Bin_4(2) == 0)
        {
            bins_4_number = bins_4_number + 1;
            SendData();
            Serial2.print("p0.pic=4"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"OK\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            ServoControl_OPEN(4);
            waiting(1);
            ServoControl_CLOSE(4);
        }
        else
        {
            SendData();
            Serial2.print("p0.pic=4"); //显示照片
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.print("t9.txt=\"NO\""); //分类是否成功
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            Serial2.write(0XFF);
            waiting(2);
            //其它垃圾已满
        }
    }
    break;
    default:
        break;
    }
}

//超声波传感器初始化
void HCSR04_Init()
{

    //初始化1号超声波传感器引脚
    pinMode(trigPin_1, OUTPUT);
    pinMode(echoPin_1, INPUT);

    //初始化2号超声波传感器引脚
    pinMode(trigPin_2, OUTPUT);
    pinMode(echoPin_2, INPUT);

    //初始化3号超声波传感器引脚
    pinMode(trigPin_3, OUTPUT);
    pinMode(echoPin_3, INPUT);

    //初始化4号超声波传感器引脚
    pinMode(trigPin_4, OUTPUT);
    pinMode(echoPin_4, INPUT);

    Overflow_1_state = 0;
    Overflow_2_state = 0;
    Overflow_3_state = 0;
    Overflow_4_state = 0;

    bins_1_number = 0;
    bins_2_number = 0;
    bins_3_number = 0;
    bins_4_number = 0;

    count = 0;

    digitalWrite(trigPin_1, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_1, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_1, LOW);
    bins_1_duration = pulseIn(echoPin_1, HIGH);
    bins_1_distancemax = (bins_1_duration / 2) / 29.1;

    digitalWrite(trigPin_2, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_2, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_2, LOW);
    bins_2_duration = pulseIn(echoPin_2, HIGH);
    bins_2_distancemax = (bins_2_duration / 2) / 29.1;

    digitalWrite(trigPin_3, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_3, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_3, LOW);
    bins_3_duration = pulseIn(echoPin_3, HIGH);
    bins_3_distancemax = (bins_3_duration / 2) / 29.1;

    digitalWrite(trigPin_4, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin_4, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin_4, LOW);
    bins_4_duration = pulseIn(echoPin_4, HIGH);
    bins_4_distancemax = (bins_4_duration / 2) / 29.1;
}

//压力传感器初始化
void HX711_Init()
{

    scale_R.begin(dataPin_R, clockPin_R);
    scale_K.begin(dataPin_K, clockPin_K);
    scale_T.begin(dataPin_T, clockPin_T);
    scale_O.begin(dataPin_O, clockPin_O);

    // loadcell factor 5 KG
    scale_R.set_scale(511);
    scale_K.set_scale(511);
    scale_T.set_scale(511);
    scale_O.set_scale(511);

    // reset the scale to 0
    scale_R.tare();
    scale_K.tare();
    scale_T.tare();
    scale_O.tare();
}

//舵机初始化
void Servo_Init()
{

    servo_R_door.attach(SERVO_R_PIN, 500, 2500); // 将13引脚与声明的舵机对象连接起来
    servo_K_door.attach(SERVO_K_PIN, 500, 2500); // 将44引脚与声明的舵机对象连接起来
    servo_T_door.attach(SERVO_T_PIN, 500, 2500); // 将3引脚与声明的舵机对象连接起来
    servo_O_door.attach(SERVO_O_PIN, 500, 2500); // 将8引脚与声明的舵机对象连接起来

    ServoControl_CLOSE(1);
    delay(500);
    ServoControl_CLOSE(2);
    delay(500);
    ServoControl_CLOSE(3);
    delay(500);
    ServoControl_CLOSE(4);
}

void setup()
{
    Serial1.begin(19200); //19200
    Serial2.begin(9600); //9600
    Serial.begin(115200); //115200
    HCSR04_Init();
    BC20_Init();
    Servo_Init();
}

void loop()
{
    //每三秒向EMQ上传一次容量检测数据
    mqttPublish();
    waiting(3);

    if (Serial1.available())
    {
        count = count + 1;
        Serial2.print("page display"); //停止播放视频
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Bins_state = Serial1.read();
        runCmd(int(Bins_state));
        Serial2.print("page video"); //继续播放视频
        Serial2.write(0XFF);
        Serial2.write(0XFF);
        Serial2.write(0XFF);
    }
    else if (Serial2.available())
    {
        SendData();
    }
}